!####################################################################################################
! MODULE toolbox_own
!
! This module file contains all subroutines+ functions that are not project-
! specific and can thus be used across multiple projects
!
! Author: Annika Bacher (annika.bacher@eui.eu) -- some codes + subroutines from:
! This code was copied and adapted from the source codes accompanying the book
! Fehr, H. & Kindermann, F. (2018). Introduction to Computational Economics using Fortran. 
!         " Oxford: Oxford University Press.     
!
! August 2019
!
!####################################################################################################

module toolbox_own


contains 

!##############################################################################  

!                           INCLUDED FUNCTIONS  
  
!##############################################################################  

    ! 1) function to count elements in array smaller than y
       
        function cntarray(x,nxx,y)

            implicit none

            integer :: nxx, i, cntarray
            real*8 :: x(nxx),y

            cntarray = 0

            do  i=1,nxx
            if(x(i) <= y) then
            cntarray = cntarray+1
            end if
            end do

            end function cntarray


    ! 2) cumulative sum
    
        function cumsum(x,length)
        
        implicit none

        integer :: length, i
        
        real*8  :: x(length), cumsum(length)
        
        cumsum(1) = x(1)
        
        do i=2,length
        
        cumsum(i) = cumsum(i-1) + x(i)
        
        end do
        
        end function cumsum
        
        
    ! 3) random normally distributed variables
    
        function rand_normal(mean, stdev) result(c)
        
        implicit none
        
        real*8 :: mean, stdev, c, temp(2), theta, rs
        
        real(8),  parameter :: PI  = 4 * atan (1.0_8)
        
        call random_number(temp)
        rs=(-2.0d0*log(temp(1)))**0.5
        theta = 2.0d0*PI*temp(2)
        c= mean+stdev*rs*sin(theta)
        
        
        end function
    
    
    ! 4) returns mean of an array
    
        function mean(x,nxx) 
        
        implicit none
        
        integer :: nxx
        
        real*8 :: x(nxx), mean
        
        mean = sum(x)/nxx
        
        
        end function 
        
    ! 5) returns mean of an array w/o entries that are -inf
    
        function mean_noinf(x,nxx,inf) 
        
        implicit none
        
        integer :: nxx, t, i 
        
        real*8 :: x(nxx), mean_noinf, inf, sumx
        
        t= 0
        
        sumx = 0d0
        
        do i=1,nxx
        
        if(x(i) /= -inf) then
        
        t= t+1
        sumx = sumx+x(i)
        end if 
        
        end do 
        
        if(t>0d0) then
        mean_noinf = sumx/t
        else 
        mean_noinf = 0d0
        end if 
        
        end function 
 
    ! 6) set of functions that checks ro equality of integers
 
    function assert_eq2(n1, n2, string)
    
    !##############################################################################
    ! FUNCTION assert_eq2
    !
    ! Checks equality for two integers.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     Press, W.H., Teukolsky, S.A., Vetterling, W.T. & Flannery, B.P. (1992). 
    !     Numerical Recipes in Fortran 90: The Art of Parallel Scientific
    !     Computing, 2nd edition. Cambridge: Cambridge University Press.
    !##############################################################################
    
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! integers to compare for equality
        integer, intent(in) :: n1, n2
        
        ! routine from which error should be thrown
        character(len=*), intent(in) :: string
        
        ! return value
        integer :: assert_eq2
        
        
        !##### ROUTINE CODE #######################################################
        
        ! if equality, set return value to n1
        if (n1 == n2)then
            assert_eq2 = n1
        
        ! else throw error message
        else
            call error(string, 'an assertion failed in assert_eq2')
        end if
    
    end function assert_eq2
    
   
    function assert_eq3(n1, n2, n3, string)
    
     
    !##############################################################################
    ! FUNCTION assert_eq3
    !
    ! Checks equality for three integers.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     Press, W.H., Teukolsky, S.A., Vetterling, W.T. & Flannery, B.P. (1992). 
    !     Numerical Recipes in Fortran 90: The Art of Parallel Scientific
    !     Computing, 2nd edition. Cambridge: Cambridge University Press.
    !##############################################################################
    
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! integers to compare for equality
        integer, intent(in) :: n1, n2, n3
        
        ! routine from which error should be thrown
        character(len=*), intent(in) :: string
        
        ! return value
        integer :: assert_eq3
        
        
        !##### ROUTINE CODE #######################################################
        
        ! if equality, set return value to n1
        if (n1 == n2 .and. n2 == n3)then
            assert_eq3 = n1
        
        ! else throw error message
        else
            call error(string, 'an assertion failed in assert_eq3')
        end if
    
    end function assert_eq3

   
    function assert_eq4(n1, n2, n3, n4, string)
    
     !##############################################################################
    ! FUNCTION assert_eq4
    !
    ! Checks equality for four integers.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     Press, W.H., Teukolsky, S.A., Vetterling, W.T. & Flannery, B.P. (1992). 
    !     Numerical Recipes in Fortran 90: The Art of Parallel Scientific
    !     Computing, 2nd edition. Cambridge: Cambridge University Press.
    !##############################################################################
    
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! integers to compare for equality
        integer, intent(in) :: n1, n2, n3, n4
        
        ! routine from which error should be thrown
        character(len=*), intent(in) :: string
        
        ! return value
        integer :: assert_eq4
        
        
        !##### ROUTINE CODE #######################################################
        
        ! if equality, set return value to n1
        if (n1 == n2 .and. n2 == n3 .and. n3 == n4)then
            assert_eq4 = n1
        
        ! else throw error message
        else
            call error(string, 'an assertion failed in assert_eq4')
        end if
    
    end function assert_eq4
    

    function assert_eq5(n1, n2, n3, n4, n5, string)
    
        
    !##############################################################################
    ! FUNCTION assert_eq5
    !
    ! Checks equality for five integers.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     Press, W.H., Teukolsky, S.A., Vetterling, W.T. & Flannery, B.P. (1992). 
    !     Numerical Recipes in Fortran 90: The Art of Parallel Scientific
    !     Computing, 2nd edition. Cambridge: Cambridge University Press.
    !##############################################################################
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! integers to compare for equality
        integer, intent(in) :: n1, n2, n3, n4, n5
        
        ! routine from which error should be thrown
        character(len=*), intent(in) :: string
        
        ! return value
        integer :: assert_eq5
        
        
        !##### ROUTINE CODE #######################################################
        
        ! if equality, set return value to n1
        if (n1 == n2 .and. n2 == n3 .and. n3 == n4 .and. n4 == n5)then
            assert_eq5 = n1
        
        ! else throw error message
        else
            call error(string, 'an assertion failed in assert_eq5')
        end if
    
    end function assert_eq5


    function assert_eqn(nn, string)
    
    !##############################################################################
    ! FUNCTION assert_eqn
    !
    ! Checks equality for n integers.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     Press, W.H., Teukolsky, S.A., Vetterling, W.T. & Flannery, B.P. (1992). 
    !     Numerical Recipes in Fortran 90: The Art of Parallel Scientific
    !     Computing, 2nd edition. Cambridge: Cambridge University Press.
    !##############################################################################
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! integers to compare for equality
        integer, intent(in) :: nn(:)
        
        ! routine from which error should be thrown
        character(len=*), intent(in) :: string
        
        ! return value
        integer :: assert_eqn
        
        
        !##### ROUTINE CODE #######################################################
        
        ! if equality, set return value to n1
        if (all(nn(2:) == nn(1)))then
            assert_eqn = nn(1)
        
        ! else throw error message
        else
            call error(string, 'an assertion failed in assert_eqn')
        end if
    
    end function assert_eqn


!##############################################################################  

!                           INCLUDED SUBROUTINES 
  
!##############################################################################  

  ! discretization of one-dimensional normal distribution 

subroutine normal_discrete_1(x, prob, mu, sigma)
    
    
    !##############################################################################
    ! SUBROUTINE normal_discrete_1
    !
    ! Creates n points and probabilities for a normal distribution.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     CompEcon toolbox of Mario Miranda and Paul Fackler available under
    !     http://www4.ncsu.edu/~pfackler/compecon/toolbox.html
    !
    ! REFERENCE: Miranda, M. & Fackler, P. (2002). Applied Computational Economics 
    !            and Finance. Cambridge: MIT Press.
    !##############################################################################
     
        implicit none
     
     
        !##### INPUT/OUTPUT VARIABLES #############################################
     
        ! discrete points of normal distribution
        real*8, intent(out) :: x(:)
     
        ! probability weights
        real*8, intent(out) :: prob(:)
     
        ! expectation of distribution
        real*8, optional :: mu
     
        ! variance of distribution
        real*8, optional :: sigma
     
     
        !##### OTHER VARIABLES ####################################################
     
        real*8 :: mu_c, sigma_c, pim4, z=0d0, z1, p1, p2, p3, pp
        integer :: n, m, i, j, its
        integer, parameter :: maxit = 200
        real*8, parameter :: pi = 3.1415926535897932d0
     
     
        !##### ROUTINE CODE #######################################################
     
        ! initialize parameters
        mu_c = 0d0
        if(present(mu))mu_c = mu
        sigma_c = 1d0
        if(present(sigma))sigma_c = sqrt(sigma)
        
        if(sigma_c < 0d0)then
            call error('normal_discrete','sigma has negative value')
        endif
     
        ! check for right array sizes
        n = assert_eq2(size(x,1), size(prob,1), 'normal_discrete')
     
        ! calculate 1/pi^0.25
        pim4 = 1d0/pi**0.25d0
     
        ! get number of points
        m = (n+1)/2
     
        ! initialize x and prob
        x = 0d0
        prob = 0d0
     
        ! start iteration
        do i = 1, m
     
            ! set reasonable starting values
            if(i == 1)then
                z = sqrt(dble(2*n+1))-1.85575d0*(dble(2*n+1)**(-1d0/6d0))
            elseif(i == 2)then
                z = z - 1.14d0*(dble(n)**0.426d0)/z
            elseif(i == 3)then
                z = 1.86d0*z+0.86d0*x(1)
            elseif(i == 4)then
                z = 1.91d0*z+0.91d0*x(2);
            else
                z = 2d0*z+x(i-2);
            endif
     
            ! root finding iterations
            its = 0
            do while(its < maxit)
                its = its+1
                p1 = pim4
                p2 = 0d0
                do j = 1, n
                    p3 = p2
                    p2 = p1
                    p1 = z*sqrt(2d0/dble(j))*p2-sqrt(dble(j-1)/dble(j))*p3
                enddo
                pp = sqrt(2d0*dble(n))*p2
                z1 = z
                z  = z1-p1/pp
                if(abs(z-z1) < 1e-14)exit
            enddo
            if(its >= maxit)then
                call error('normal_discrete', &
                    'Could not discretize normal distribution')
            endif
            x(n+1-i) = z
            x(i) = -z
            prob(i) = 2d0/pp**2
            prob(n+1-i) = prob(i)
        enddo
     
        ! set output data
        prob = prob/sqrt(pi)
        x = x*sqrt(2d0)*sigma_c + mu_c
     
    end subroutine normal_discrete_1


        ! discretization of two-dimensional normal distribution
    
    
subroutine normal_discrete_2(n, x, prob, mu, sigma, rho)
    
    !##############################################################################
    ! SUBROUTINE normal_discrete_2
    !
    ! Creates n1*n2 points and probabilities for a two-dimensional normal
    !     distribution.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     CompEcon toolbox of Mario Miranda and Paul Fackler available under
    !     http://www4.ncsu.edu/~pfackler/compecon/toolbox.html
    !
    ! REFERENCE: Miranda, M. & Fackler, P. (2002). Applied Computational Economics 
    !            and Finance. Cambridge: MIT Press.
    !##############################################################################
     
        implicit none
     
     
        !##### INPUT/OUTPUT VARIABLES #############################################
     
        ! number of points in every direction
        integer, intent(in) :: n(2)
     
        ! discrete points of normal distribution
        real*8, intent(out) :: x(:, :)
     
        ! probability weights
        real*8, intent(out) :: prob(:)
     
        ! expectation of distribution
        real*8, optional :: mu(2)
     
        ! variance of distribution
        real*8, optional :: sigma(2)
     
        ! correlation of distribution
        real*8, optional :: rho
     
     
        !##### OTHER VARIABLES ####################################################
     
        real*8 :: mu_c(2), sig_c(2), rho_c, sigma_c(2,2), l(2,2)
        real*8 :: x1(n(1)), x2(n(2)), p1(n(1)), p2(n(2))
        integer :: m, j, k
     
     
        !##### ROUTINE CODE #######################################################
     
        ! initialize parameters
        mu_c = 0d0
        if(present(mu))mu_c = mu
        sig_c(1) = 1d0
        if(present(sigma))sig_c = sigma
        rho_c = 0d0
        if(present(rho))rho_c = rho
     
        ! set up variance covariance matrix
        sigma_c(1, 1) = sig_c(1)
        sigma_c(2, 2) = sig_c(2)
        sigma_c(1, 2) = rho_c*sqrt(sig_c(1)*sig_c(2))
        sigma_c(2, 1) = sigma_c(1, 2)
     
        ! check for right array sizes
         m = assert_eq3(size(x,1), size(prob,1), n(1)*n(2), 'normal_discrete2')
         m = assert_eq2(size(x,2), 2, 'normal_discrete2')
     
        ! check whether sigma is symmetric
        if(any(abs(transpose(sigma_c) - sigma_c) > 1d-20)) &
            call error('normal_discrete', &
            'Variance-Covariance matrix is not symmetric')
     
        ! get standard normal distributed random variables
        call normal_discrete_1(x1, p1, 0d0, 1d0)
        call normal_discrete_1(x2, p2, 0d0, 1d0)
     
        ! get joint distribution
        m = 1
        do k = 1, n(2)
            do j = 1, n(1)
                prob(m) = p1(j)*p2(k)
                x(m, :) = (/x1(j), x2(k)/)
                m = m+1
            enddo
        enddo
     
        ! decompose var-cov matrix
        if(.not.any(abs(sig_c)  <= 1d-100))then
            call cholesky(sigma_c, l)
        else
            l = 0d0
            l(1,1) = sqrt(sig_c(1))
            l(2,2) = sqrt(sig_c(2))
        endif
     
        ! calculate distribution
        x = matmul(x, transpose(l))
        x(:, 1) = x(:, 1) + mu_c(1)
        x(:, 2) = x(:, 2) + mu_c(2)
     
    end subroutine normal_discrete_2

        ! discretize interval using rouwenhorst method
        
subroutine rouwenhorst(rho,sigma_eps,mu_eps,n,zgrid,P)

!rouwenhorst.f90
!
![zgrid, P] = rouwenhorst(rho, sigma_eps, n)
!
! rho is the 1st order autocorrelation
! sigma_eps is the standard deviation of the error term
! n is the number of points in the discrete approximation
! mu_eps is the mean of the process
!
! see "Finite State Markov-chain Approximations to Highly Persistent
! Processes"


implicit none

INTEGER, INTENT(IN) :: n
DOUBLE PRECISION, INTENT(IN) :: rho,sigma_eps
DOUBLE PRECISION, DIMENSION(n,n), INTENT(OUT) :: P
DOUBLE PRECISION, DIMENSION(n), INTENT(OUT) :: zgrid

INTEGER :: i, status
DOUBLE PRECISION :: mu_eps, q, nu
DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: P11, P12, P21, P22
!EXTERNAL :: linspace


q = (rho+1.d0)/2.d0
nu = SQRT((DBLE(n)-1.d0)/(1.d0-rho**2.d0)) * sigma_eps
P = 0.d0

P(1,1) = q
P(1,2) = 1 - q
P(2,1) = 1 - q
P(2,2) = q

DO i=2,n-1
   !WRITE(*,*) i
   ALLOCATE(P11(i+1,i+1), P12(i+1,i+1), P21(i+1,i+1), P22(i+1,i+1), STAT=status)
   
   P11 = 0.d0
   P12 = 0.d0
   P21 = 0.d0
   P22 = 0.d0
 
   P11(1:i,1:i) = P(1:i,1:i)
   P12(1:i,2:i+1) = P(1:i,1:i)
   P21(2:i+1,1:i) = P(1:i,1:i)
   P22(2:i+1,2:i+1) = P(1:i,1:i)
   !WRITE(*,*) 'Assigned partial matrices'   

   P(1:i+1,1:i+1) = q*P11 + (1-q)*P12 + (1-q)*P21 + q*P22

   P(2:i,:) = P(2:i,:)/2.d0

   DEALLOCATE(P11,P12,P21,P22, STAT = status)
END DO

call grid_Cons_Equi(zgrid,mu_eps/(1.d0-rho)-nu,mu_eps/(1.d0-rho)+nu)

end subroutine


          ! discretize a VAR(1) with mean zero

subroutine discr_VAR(rhomat,cove,n,m,zgrid,P)

          ! subroutine to discretize and VAR(1) process
          ! included number of processes is flexible
          ! restriction: number of discretized states have to be
          ! the same for all included processes
          ! mean of all processes is zero
          ! INPUT ARGUMENTS:
          ! rhomat = matrix of persistence coefficients
          ! cove   = variance/ covariance matrix of the error term
          ! n      = number of discretized states
          ! m      = number of processes
          ! OUTPUT ARGUMENTS:
          ! zgrid  = discretized states
          ! P      = transition matrix

          implicit none 
          
          integer, intent(in) :: n, m
          real*8, dimension(m,m), intent(in)  :: rhomat, cove
          real*8, dimension(n**m,n**m), intent(out) :: P 
          real*8, dimension(m,n), intent(out) :: zgrid
          
          integer :: i, it_max, k,l,j
          real*8, dimension(m,m) :: Q, QT, rhomat_til, cove_til, help, help2
          real*8, dimension(m,n) ::  zgrid_til
          real*8, dimension(n,n*m) :: P_til
          real*8, dimension(n,n) :: P_til1
          real*8, dimension(n**m,n**m) :: P1, P2 
          real*8  :: d(m), muvar, zgrid_til1(n)
          
          ! transform the process
          
          it_max = 1000
          
          call jacobi_eigenvalue(m,cove,it_max, Q, d)

          QT = transpose(Q)
          
          help = matmul(QT,rhomat)
          rhomat_til = matmul(help,Q)
          help2 = matmul(Q,cove)
          cove_til   = matmul(help2,QT)
          
          !write(*,*) Q, QT, cove_til
          
          ! discretize the transformed processes independently  
         
          muvar = 0d0
          
          do i=1,m
          call rouwenhorst(rhomat_til(i,i),sqrt(cove_til(i,i)),muvar,n,zgrid_til1,P_til1)
          !write(*,*) zgrid_til1 
          zgrid_til(i,:) = zgrid_til1
          P_til(:,(i-1)*n+1:(i-1)*n+n) = P_til1 
          end do

          ! recover original process
          zgrid = matmul(Q,zgrid_til)
          
          ! kron of transition matrix (only for m=2) -- expand for more states
          
          do i =1,n
            do j = 1,n
            
          P1((i-1)*n+1:(i-1)*n+n,(j-1)*n+1:(j-1)*n+n) = P_til(i,j)
          
            do k=1,n
            do l=1,n
          P2(i+(l-1)*n,j+(k-1)*n) = P_til(i,j+n)
            end do
            end do
           
            end do
          end do
          
          P = P1*P2
          
         ! write(*,*) P, zgrid

end subroutine 



        ! find root of one-dimensional function using newton method
        
subroutine newton_interpol(x, funcv, check_return)
    
        implicit none
     
     
        !##### INPUT/OUTPUT VARIABLES #############################################
     
        ! initial guess and root of the function
        real*8, intent(inout) :: x
     
        ! check is true if broydn converged to local minimum or can make no
        !     further progress
        logical, intent(out), optional :: check_return
     
        !##### OTHER VARIABLES ####################################################
     
        real*8, parameter :: eps = epsilon(x)
        real*8 :: tolf, tolmin
        real*8, parameter :: tolx = eps
        real*8, parameter :: stpmx = 100d0
        real*8 :: x1, x2, f1, f2, xnew, fnew, h
        integer :: its
     
     
        !##### INTERFACES #########################################################
     
        ! interface for the function
        interface
            function funcv(p)
                implicit none
                real*8, intent(in) :: p
                real*8 :: funcv
            end function funcv
        end interface
     
     
        !##### ROUTINE CODE #######################################################
     
        ! set tolerance levels
        tolf = 1d-8
        tolmin = 1d-8
        if(present(check_return))check_return = .false.
     
        ! initialize values
        x1 = x
        h = 1d-6*max(abs(x), 0.01d0)
        x2 = x + h
     
        ! calculate function values at x1, x2
        f1 = funcv(x1)
        f2 = funcv(x2)
     
        ! check if already in zero
        if(abs(f1) < 0.01d0*tolf)then
            x = x1
            if(present(check_return))check_return = .false.
            return
        endif
     
        if(abs(f2) < 0.01d0*tolf)then
            x = x2
            if(present(check_return))check_return = .false.
            return
        endif
     
        ! start iteration
        do its = 1, 200
     
            ! calculate new point xnew
            xnew = x2 - (x2-x1)/(1d0-f1/f2)
     
            ! calculate new function value
            fnew = funcv(xnew)
     
            ! check wether function is small enough
            if(abs(fnew) < tolf)then
                x = xnew
                if(present(check_return))check_return = .false.
                return
            endif
     
            ! check whether you are in a minimum or cannot proceed further
            if(2d0*abs(xnew-x2) < tolx*abs(xnew+x2))then
                x = x2
                if(present(check_return))check_return = .true.
                return
            endif
     
            ! else set new data and repeat step
            x1 = x2
            f1 = f2
            x2 = xnew
            f2 = fnew

            if(abs((1d0-f1/f2)/(x2-x1)) < tolmin .or. &
               abs((1d0-f1/f2)) < epsilon(1d0))then
                x = x1
                if(present(check_return))check_return = .true.
                return
            endif
        enddo
     
        ! throw warning if newton didn't converge
        if(present(check_return))check_return = .true.
     
        x = xnew
    
    end subroutine newton_interpol
    
    
        ! Constructs a whole equidistant grid on [left,right]
   
subroutine grid_Cons_Equi(a, left, right)
     
     
        !##### INPUT/OUTPUT VARIABLES #############################################
     
        ! the array to fill
        real*8, intent(out) :: a(1:)
     
        ! left and right interval point
        real*8, intent(in) :: left, right
     
     
        !##### OTHER VARIABLES ####################################################
     
        real*8 :: h
        integer :: j, n
     
     
        !##### ROUTINE CODE #######################################################
        
        ! get number of grid points
        n = size(a, 1)-1
     
        ! check for left <= right
        if(left >= right)call error('grid_Cons_Equi', &
            'left interval point greater than right point')
     
        ! calculate distance between grid points
        h = (right-left)/dble(n)
     
        ! calculate grid value
        a = h*(/(dble(j), j=0,n)/)+left
     
end subroutine grid_Cons_Equi


        ! Produces error message and stops the program

subroutine error(routine, message)
    
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! routine in which error occured
        character(len=*), intent(in) :: routine
        
        ! error message
        character(len=*), intent(in) :: message
        
        
        !##### ROUTINE CODE #######################################################
        
        ! write error message
        write(*,'(/a,a,a,a/)')'ERROR ',routine,': ',message
        
        ! stop program
        stop
    
end subroutine error


        ! subroutine for computing eigenvalues & eigenvectors of a NxN matrix
        ! taken from: https://people.sc.fsu.edu/~jburkardt/f_src/jacobi_eigenvalue/jacobi_eigenvalue.f90


subroutine jacobi_eigenvalue ( n, a, it_max, v, d)

!*****************************************************************************80
!
!! JACOBI_EIGENVALUE carries out the Jacobi eigenvalue iteration.
!
!  Discussion:
!
!    This function computes the eigenvalues and eigenvectors of a
!    real symmetric matrix, using Rutishauser's modfications of the classical
!    Jacobi rotation method with threshold pivoting. 
!
!  Licensing:
!
!    This code is distributed under the GNU LGPL license.
!
!  Modified:
!
!    17 September 2013
!
!  Author:
!
!    FORTRAN90 version by John Burkardt
!
!  Parameters:
!
!    Input, integer ( kind = 4 ) N, the order of the matrix.
!
!    Input, real ( kind = 8 ) A(N,N), the matrix, which must be square, real,
!    and symmetric.
!
!    Input, integer ( kind = 4 ) IT_MAX, the maximum number of iterations.
!
!    Output, real ( kind = 8 ) V(N,N), the matrix of eigenvectors.
!
!    Output, real ( kind = 8 ) D(N), the eigenvalues, in descending order.
!
!    Output, integer ( kind = 4 ) IT_NUM, the total number of iterations.
!
!    Output, integer ( kind = 4 ) ROT_NUM, the total number of rotations.
!
  implicit none

  integer ( kind = 4 ) n

  real ( kind = 8 ) a(n,n)
  real ( kind = 8 ) bw(n)
  real ( kind = 8 ) c
  real ( kind = 8 ) d(n)
  real ( kind = 8 ) g
  real ( kind = 8 ) gapq
  real ( kind = 8 ) h
  integer ( kind = 4 ) i
  integer ( kind = 4 ) it_max
  integer ( kind = 4 ) it_num
  integer ( kind = 4 ) j
  integer ( kind = 4 ) k
  integer ( kind = 4 ) l
  integer ( kind = 4 ) m
  integer ( kind = 4 ) p
  integer ( kind = 4 ) q
  integer ( kind = 4 ) rot_num
  real ( kind = 8 ) s
  real ( kind = 8 ) t
  real ( kind = 8 ) tau
  real ( kind = 8 ) term
  real ( kind = 8 ) termp
  real ( kind = 8 ) termq
  real ( kind = 8 ) theta
  real ( kind = 8 ) thresh
  real ( kind = 8 ) v(n,n)
  real ( kind = 8 ) w(n)
  real ( kind = 8 ) zw(n)

  do j = 1, n
    do i = 1, n
      v(i,j) = 0.0D+00
    end do
    v(j,j) = 1.0D+00
  end do

  do i = 1, n
    d(i) = a(i,i)
  end do

  bw(1:n) = d(1:n)
  zw(1:n) = 0.0D+00
  it_num = 0
  rot_num = 0

  do while ( it_num < it_max )

    it_num = it_num + 1
!
!  The convergence threshold is based on the size of the elements in
!  the strict upper triangle of the matrix.
!
    thresh = 0.0D+00
    do j = 1, n
      do i = 1, j - 1
        thresh = thresh + a(i,j) ** 2
      end do
    end do

    thresh = sqrt ( thresh ) / real ( 4 * n, kind = 8 )

    if ( thresh == 0.0D+00 ) then
      exit 
    end if

    do p = 1, n
      do q = p + 1, n

        gapq = 10.0D+00 * abs ( a(p,q) )
        termp = gapq + abs ( d(p) )
        termq = gapq + abs ( d(q) )
!
!  Annihilate tiny offdiagonal elements.
!
        if ( 4 < it_num .and. &
             termp == abs ( d(p) ) .and. &
             termq == abs ( d(q) ) ) then

          a(p,q) = 0.0D+00
!
!  Otherwise, apply a rotation.
!
        else if ( thresh <= abs ( a(p,q) ) ) then

          h = d(q) - d(p)
          term = abs ( h ) + gapq

          if ( term == abs ( h ) ) then
            t = a(p,q) / h
          else
            theta = 0.5D+00 * h / a(p,q)
            t = 1.0D+00 / ( abs ( theta ) + sqrt ( 1.0D+00 + theta * theta ) )
            if ( theta < 0.0D+00 ) then 
              t = - t
            end if
          end if

          c = 1.0D+00 / sqrt ( 1.0D+00 + t * t )
          s = t * c
          tau = s / ( 1.0D+00 + c )
          h = t * a(p,q)
!
!  Accumulate corrections to diagonal elements.
!
          zw(p) = zw(p) - h                  
          zw(q) = zw(q) + h
          d(p) = d(p) - h
          d(q) = d(q) + h

          a(p,q) = 0.0D+00
!
!  Rotate, using information from the upper triangle of A only.
!
          do j = 1, p - 1
            g = a(j,p)
            h = a(j,q)
            a(j,p) = g - s * ( h + g * tau )
            a(j,q) = h + s * ( g - h * tau )
          end do

          do j = p + 1, q - 1
            g = a(p,j)
            h = a(j,q)
            a(p,j) = g - s * ( h + g * tau )
            a(j,q) = h + s * ( g - h * tau )
          end do

          do j = q + 1, n
            g = a(p,j)
            h = a(q,j)
            a(p,j) = g - s * ( h + g * tau )
            a(q,j) = h + s * ( g - h * tau )
          end do
!
!  Accumulate information in the eigenvector matrix.
!
          do j = 1, n
            g = v(j,p)
            h = v(j,q)
            v(j,p) = g - s * ( h + g * tau )
            v(j,q) = h + s * ( g - h * tau )
          end do

          rot_num = rot_num + 1

        end if

      end do
    end do

    bw(1:n) = bw(1:n) + zw(1:n)
    d(1:n) = bw(1:n)
    zw(1:n) = 0.0D+00

  end do
!
!  Restore upper triangle of input matrix.
!
  do j = 1, n
    do i = 1, j - 1
      a(i,j) = a(j,i)
    end do
  end do
!
!  Ascending sort the eigenvalues and eigenvectors.
!
  do k = 1, n - 1

    m = k

    do l = k + 1, n
      if ( d(l) < d(m) ) then
        m = l
      end if
    end do

    if ( m /= k ) then

      t    = d(m)
      d(m) = d(k)
      d(k) = t

      w(1:n)   = v(1:n,m)
      v(1:n,m) = v(1:n,k)
      v(1:n,k) = w(1:n)

    end if

  end do

  return


end subroutine

        ! subroutine to calculate cholesky characterization of a symmetric matrix

subroutine cholesky(a, l)
    
     !##############################################################################
    ! SUBROUTINE cholesky
    !
    ! Calculates cholesky factorization of a symmetric matrix.
    !
    ! PARTS OF THIS PROCEDURE WERE COPIED AND ADAPTED FROM:
    !     Press, W.H., Teukolsky, S.A., Vetterling, W.T. & Flannery, B.P. (1992). 
    !     Numerical Recipes in Fortran 90: The Art of Parallel Scientific
    !     Computing, 2nd edition. Cambridge: Cambridge University Press.
    !##############################################################################
    
    
        !##### INPUT/OUTPUT VARIABLES #############################################
        
        ! the matrix that should be decomposed
        real*8, intent(in) :: a(:, :)
        
        ! the cholesky factor
        real*8, intent(out) :: l(:, :)
        
        
        !##### OTHER VARIABLES ####################################################
        
        integer :: i, n
        real*8 :: summ, p(size(a,1))
        
        
        !##### ROUTINE CODE #######################################################
        
        ! assert equalities
        n = assert_eq5(size(a,1), size(a,2), size(l, 1), size(l, 2), &
            size(p), 'cholesky')
        
        ! copy matrix
        l = a
        
        ! decompose matrix
        do i = 1, n
            summ = l(i,i)-dot_product(l(i,1:i-1), l(i,1:i-1))
            if(summ <= 0d0)call error('cholesky', &
                'Cholesky decomposition failed')
            p(i) = sqrt(summ)
            l(i+1:n,i) = (l(i,i+1:n)-matmul(l(i+1:n,1:i-1),l(i,1:i-1)))/p(i)
        enddo
        
        ! copy matrix
        do i = 1, n
            l(i, i) = p(i)
            l(i, i+1:n) = 0d0
        enddo
    
    end subroutine cholesky
    
       SUBROUTINE init_random_seed()
            INTEGER :: i, n, clock
            INTEGER, DIMENSION(:), ALLOCATABLE :: seed
          
            CALL RANDOM_SEED(size = n)
            ALLOCATE(seed(n))
          
            CALL SYSTEM_CLOCK(COUNT=clock)
          
            seed = clock + 37 * (/ (i - 1, i = 1, n) /)
            CALL RANDOM_SEED(PUT = seed)
          
            DEALLOCATE(seed)
         END SUBROUTINE

end module
